#!/usr/bin/python3
# coding=utf8

import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped

from dynamic_reconfigure.client import Client as DynamicReconfigureClient

def get_current_height(tf_buffer):
    try:
        # 获取 /map 到 /body 的变换
        transform: TransformStamped = tf_buffer.lookup_transform("map", "body", rospy.Time(0), rospy.Duration(1.0))
        current_height = transform.transform.translation.z
        return current_height
    except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
        rospy.logerr(f"TF lookup failed: {e}")
        return None

def update_pointcloud_to_laserscan_params(current_height, min_offset, max_offset):
    # 计算新的高度范围
    min_height = current_height - min_offset
    max_height = current_height + max_offset

    # 使用 dynamic_reconfigure 更新参数
    client = DynamicReconfigureClient("/pointcloud_to_laserscan")
    params = {
        "min_height": min_height,
        "max_height": max_height
    }
    client.update_configuration(params)

def main():
    rospy.init_node("dynamic_height_filter")

    # 初始化 TF 缓冲区和监听器
    tf_buffer = tf2_ros.Buffer()
    tf_listener = tf2_ros.TransformListener(tf_buffer)

    # 用户定义的偏移量
    min_offset = rospy.get_param("~min_offset", 0.1)
    max_offset = rospy.get_param("~max_offset", 0.1)

    rate = rospy.Rate(10)  # 10 Hz
    while not rospy.is_shutdown():
        # 获取当前高度
        current_height = get_current_height(tf_buffer)
        if current_height is not None:
            # 更新 pointcloud_to_laserscan 参数
            update_pointcloud_to_laserscan_params(current_height, min_offset, max_offset)
        
        rate.sleep()

if __name__ == "__main__":
    main()